#ifndef PoseModifierHelper_h
#define PoseModifierHelper_h

#include "../CharacterInstance.h"


namespace PoseModifierHelper 
{

	ILINE ISkeletonPose* GetISkeletonPose(const SAnimationPoseModiferParams& params)
	{
		return static_cast<CCharInstance*>(params.pCharacterInstance)->CCharInstance::GetISkeletonPose();
	}

	// NOTE: Get() helpers that don't return an interface should never be used!
	// Only here for backward-compatibility.

	ILINE CCharInstance* GetCharInstance(const SAnimationPoseModiferParams& params)
	{
		return static_cast<CCharInstance*>(params.pCharacterInstance);
	}

	ILINE CSkeletonPose* GetSkeletonPose(const SAnimationPoseModiferParams& params)
	{
		return &(static_cast<CCharInstance*>(params.pCharacterInstance)->m_SkeletonPose);
	}

	ILINE CSkeletonAnim* GetSkeletonAnim(const SAnimationPoseModiferParams& params)
	{
		return &(static_cast<CCharInstance*>(params.pCharacterInstance)->m_SkeletonAnim);
	}

	ILINE CModelSkeleton* GetModelSkeleton(const SAnimationPoseModiferParams& params)
	{
		ISkeletonPose *pSkeletonPose = static_cast<CCharInstance*>(params.pCharacterInstance)->CCharInstance::GetISkeletonPose();	
		CModelSkeleton* pModelSkeleton = static_cast<CSkeletonPose*>( pSkeletonPose )->m_pModelSkeleton;	
		return pModelSkeleton;
	}

	ILINE void ComputeAbsoluteTransformation(const SAnimationPoseModiferParams& params, uint32 index = 0)
	{
		CModelSkeleton* pModelSkeleton = PoseModifierHelper::GetModelSkeleton(params);
		if (!pModelSkeleton)
			return;

		for (uint32 i=index; i<params.jointCount; ++i)
		{
			uint32 p = pModelSkeleton->m_arrModelJoints[i].m_idxParent;
			params.pPoseAbsolute[i]	= params.pPoseAbsolute[p] * params.pPoseRelative[i];
		}
	}

	ILINE void SetJointAbsolute(const SAnimationPoseModiferParams& params, uint32 index, const Quat* pOrientation, const Vec3* pPosition)
	{
		QuatT& pose = params.pPoseAbsolute[index];
		if (pOrientation)
			pose.q = *pOrientation;
		if (pPosition)
			pose.t = *pPosition;

		int32 parentIndex = params.GetICharacterModelSkeleton()->GetJointParentIDByID(index);
		if (parentIndex > -1)
		{
			params.pPoseRelative[index].t = (pose.t - params.pPoseAbsolute[parentIndex].t) * params.pPoseAbsolute[parentIndex].q;
			params.pPoseRelative[index].q = params.pPoseAbsolute[parentIndex].q.GetInverted() * pose.q;
		}

		ComputeAbsoluteTransformation(params, index);
	}

	void IK_Solver2Bones(const Vec3r& goal, const IKLimbType& rIKLimbType, QuatT* const __restrict pRelativePose, QuatT* const __restrict pAbsolutePose);
	void IK_Solver3Bones(const Vec3r& goal, const IKLimbType& rIKLimbType, QuatT* const __restrict pRelativePose, QuatT* const __restrict pAbsolutePose);
	void IK_Solver3Bones_Old(const Vec3r& goal, const IKLimbType& rIKLimbType, QuatT* const __restrict pRelativePose, QuatT* const __restrict pAbsolutePose);
	void IK_SolverCCD(const Vec3& goal, const IKLimbType& rIKLimbType, QuatT* const __restrict pRelativePose, QuatT* const __restrict pAbsolutePose);

	int32 IK_Solver(
		const CModelSkeleton* __restrict pModelSkeleton,
		const char* strLimb, const QuatT& vLocalPos,
		QuatT* const __restrict pPoseRelative,
		QuatT* const __restrict	pPoseAbsolute,
		const bool adjustEndEffector=false);

} // namespace PoseModifierHelper

#endif // PoseModifierHelper_h
